#include <QtGui>
#include <QDebug>
#include <QDesktopWidget>
#include <QScreen>
#include <QMessageBox>
#include <QMetaEnum>

#include <base/utils/utils.h>
#include <hardware/Gps/GPS.h>
#include <hardware/Gps/utils_GPS.h>
#include <cv/highgui/VideoPOS_Manager.h>
#include <gui/controls/SvarTable.h>
#include <uav/VirtualUAV.h>
#include <uav/VirtualUAV_Quad.h>
#include <uav/VirtualUAV_Fixedwing.h>


#include "ui_controls.h"
#include "GCS_MainWindow.h"


using namespace std;
using namespace pi;


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

GCS_MainWindow::GCS_MainWindow(QWidget *parent) : QMainWindow(parent)
{
    // set this objs pointer
    SvarWithType<GCS_MainWindow*>::instance()["GCS_MainWindow.ptr"] = this;

    // show splash screen
    if( svar.GetInt("FastGCS.ui.splashScreen.show", 0) ) {
        UI_diagAbout diagAbout(NULL, 1);
        diagAbout.exec();
    }


    // set window icon
    QIcon icon(QPixmap(QString::fromUtf8(":/images/images/pis_logo.png")));
    this->setWindowIcon(icon);
    qApp->setWindowIcon(icon);

    // set initial values
    m_uasManager = NULL;
    m_comManager = NULL;
    m_dataManager = NULL;

    // get gloabl setting
    m_conf = (QSettings*) svar.GetPointer("Qt.settings.ptr", NULL);
    if( m_conf == NULL) {
        m_conf = new QSettings("FGCS.ini", QSettings::IniFormat);
    }

    // set default widget to NULL
    m_FlightIns     = NULL;
    m_infoList      = NULL;

    m_tabWidget     = NULL;
    m_mapView       = NULL;
    m_plot2D        = NULL;
    m_plot3D        = NULL;
    m_ahrsViewer    = NULL;

    // setup layout
    setupLayout();

    // load default data
    loadMesh();

    // set window minimum size
    this->setMinimumSize(1000, 700);

    // window title
    m_sbTitleString = "FastGCS";
    QString ts = m_sbTitleString + " - Link Lost";
    setWindowTitle(ts);

    // status bar
    m_sbString1 = "";
    m_sbString2 = "";
    statusBar()->hide();
    statusBar()->clearMessage();

    // create actions
    createActions();

    // create toolBars
    //createToolBars();

    // create MainMenu
    createMainMenu();

    m_systemRefreshInterval = svar.GetDouble("FastGCS.SystemRefershInterval", 0.125);     // system refresh rate
    startTimer(m_systemRefreshInterval*1000);
}

GCS_MainWindow::~GCS_MainWindow()
{
}

int GCS_MainWindow::setupLayout(void)
{
    /////////////////////////////////////////////
    /// centeral widgets
    /////////////////////////////////////////////
    m_tabWidget  = new QTabWidget(this);
    m_mapView    = new MapWidget(this);
    //m_viewSLAM   = new pi::gl::Win3D(this);

    SvarWithType<MapWidget*>::instance()["FastGCS.ui.MapWidget"] = m_mapView;


    if( svar.GetInt("FastGCS.ui.plot2D.show", 0) )      m_plot2D = new POS_Plot2D(this);
    if( svar.GetInt("FastGCS.ui.plot3D.show", 0) )      m_plot3D = new POS_Plot3D(this);
    if( svar.GetInt("FastGCS.ui.ahrsViewer.show", 0) )  m_ahrsViewer = new AHRS_GLViewer(this);

    m_tabWidget->addTab(m_mapView,    "Map");
    //m_tabWidget->addTab(m_viewSLAM,   "G-SLAM");
    if( m_plot2D != NULL )      m_tabWidget->addTab(m_plot2D, "2D Viewer");
    if( m_plot2D != NULL )      m_tabWidget->addTab(m_plot3D, "3D Viewer");
    if( m_ahrsViewer != NULL )  m_tabWidget->addTab(m_ahrsViewer, "AHRS");


    // store gloabl SLAM view
    //svar.GetPointer("MainWin3DPtr", NULL) = m_viewSLAM;

    // set initial position & zoom level
    double lat, lng;
    int    zoom;
    lat  = m_conf->value("MapWidget.lastPos.lat", 34.257287).toDouble();
    lng  = m_conf->value("MapWidget.lastPos.lng", 108.888931).toDouble();
    zoom = m_conf->value("MapWidget.lastZoom", 11).toInt();

    internals::PointLatLng pos(lat, lng);
    m_mapView->SetCurrentPosition(pos);
    m_mapView->SetZoom(zoom);
    m_mapView->setConf(m_conf);
    m_mapView->setUASManager(m_uasManager);

    svar.GetDouble("FastGCS.MapWidget.curLat", 0) = lat;
    svar.GetDouble("FastGCS.MapWidget.curLng", 0) = lng;
    svar.GetDouble("FastGCS.MapWidget.curZoom", 10) = zoom;

    #ifdef USE_INTEGRATED_SLAM
    if( svar.GetInt("slamType", 0) > 0 ) {
        mapcontrol::Map2DItem* map2ditem = new mapcontrol::Map2DItem(m_mapView->GetMapItem(), m_mapView);
    }
    #endif

    // create signal/slot connections
    connect(m_mapView, SIGNAL(OnMapDrag()),      this, SLOT(mw_onMapDrag()));
    connect(m_mapView, SIGNAL(zoomChanged(int)), this, SLOT(mw_zoomChanged(int)));

    setCentralWidget(m_tabWidget);

    /////////////////////////////////////////////
    /// right pannel (flight information)
    /////////////////////////////////////////////
    m_flightInfoPanel = new QDockWidget(this);
    m_flightInfoPanel->setAllowedAreas(Qt::LeftDockWidgetArea | Qt::RightDockWidgetArea);

    QWidget *wLeftPanelWidget = new QWidget(m_flightInfoPanel);
    QVBoxLayout *vl = new QVBoxLayout(wLeftPanelWidget);
    wLeftPanelWidget->setLayout(vl);

    m_FlightIns = new QFlightIns(wLeftPanelWidget);
    m_tabFlightInfoControl = new QTabWidget(wLeftPanelWidget);
    m_infoList  = new QKeyValueListView(wLeftPanelWidget);
    m_flightControl = new UI_flightControlPanel(wLeftPanelWidget);

    vl->addWidget(m_FlightIns, 0, Qt::AlignTop|Qt::AlignHCenter);
    vl->addWidget(m_tabFlightInfoControl,  2, 0);
    vl->setMargin(0);
    vl->setSpacing(4);

    m_tabFlightInfoControl->addTab(m_infoList, "Info");
    m_tabFlightInfoControl->addTab(m_flightControl, "Control");

    m_flightInfoPanel->setWidget(wLeftPanelWidget);
    addDockWidget(Qt::RightDockWidgetArea, m_flightInfoPanel);

    return 0;
}

void GCS_MainWindow::createActions(void)
{
    #define INSERT_ACTION_MAP(x) m_actionMap.insert(#x, x)

    /////////////////////////////////////////////
    /// file
    /////////////////////////////////////////////
    QMenu *fileMenu = new QMenu(tr("&File"));

    QAction *actOpen = new QAction(tr("Open"), this);
    actOpen->setStatusTip(tr("Open device or file"));
    actOpen->setShortcut(QKeySequence::fromString(tr("ctrl+o")));
    connect(actOpen, SIGNAL(triggered()), this, SLOT(action_Open()));
    INSERT_ACTION_MAP(actOpen);

    QAction *actClose = new QAction(tr("Close"), this);
    actClose->setStatusTip(tr("Close device or file"));
    actClose->setShortcut(QKeySequence::fromString(tr("ctrl+w")));
    connect(actClose, SIGNAL(triggered()), this, SLOT(action_Close()));
    INSERT_ACTION_MAP(actClose);

    QAction *actQuit = new QAction(tr("Quit"), this);
    actQuit->setStatusTip(tr("Quit the application"));
    actQuit->setShortcut(QKeySequence::fromString(tr("ctrl+q")));
    connect(actQuit, SIGNAL(triggered()), this, SLOT(action_Quit()));
    INSERT_ACTION_MAP(actQuit);

    fileMenu->addAction(actOpen);
    fileMenu->addAction(actClose);
    fileMenu->addSeparator();
    fileMenu->addAction(m_mapView->getAction("actionWaypoint_save"));
    fileMenu->addAction(m_mapView->getAction("actionWaypoint_load"));
    fileMenu->addAction(m_mapView->getAction("actionWaypoint_edit"));
    fileMenu->addSeparator();
    fileMenu->addAction(actQuit);

    /////////////////////////////////////////////
    /// edit
    /////////////////////////////////////////////
    QMenu *editMenu = new QMenu(tr("&Edit"));

    QAction *actSvarEdit = new QAction(tr("Svar Edit"), this);
    actSvarEdit->setStatusTip(tr("Edit Svar variables"));
    actSvarEdit->setShortcut(QKeySequence::fromString(tr("ctrl+e")));
    connect(actSvarEdit, SIGNAL(triggered()), this, SLOT(action_SvarEdit()));
    INSERT_ACTION_MAP(actSvarEdit);

    QAction *actPlaySpeed = new QAction(tr("Play Speed"), this);
    actPlaySpeed->setStatusTip(tr("Set play speed"));
    actPlaySpeed->setShortcut(QKeySequence::fromString(tr("ctrl+\\")));
    connect(actPlaySpeed, SIGNAL(triggered()), this, SLOT(action_PlaySpeed()));
    INSERT_ACTION_MAP(actPlaySpeed);


    editMenu->addAction(actSvarEdit);
    editMenu->addAction(actPlaySpeed);


    /////////////////////////////////////////////
    /// view
    /////////////////////////////////////////////
    QMenu *viewMenu = new QMenu(tr("&View"));

    QAction *actShowFullScreen = new QAction(tr("Fullscreen"), this);
    actShowFullScreen->setStatusTip(tr("Fullscreen or normal window"));
    actShowFullScreen->setCheckable(true);
    actShowFullScreen->setChecked(false);
    actShowFullScreen->setShortcut(QKeySequence::fromString(tr("f11")));
    connect(actShowFullScreen, SIGNAL(triggered()), this, SLOT(action_ShowFullScreen()));
    INSERT_ACTION_MAP(actShowFullScreen);

    QAction *actShowHideStatusBar = new QAction(tr("Statusbar"), this);
    actShowHideStatusBar->setStatusTip(tr("Show/hide statusbar"));
    actShowHideStatusBar->setCheckable(true);
    actShowHideStatusBar->setChecked(false);
    connect(actShowHideStatusBar, SIGNAL(triggered()), this, SLOT(action_ShowHideStatusBar()));
    INSERT_ACTION_MAP(actShowHideStatusBar);

    QAction *actShowHideMenuBar = new QAction(tr("Main Menu"), this);
    actShowHideMenuBar->setStatusTip(tr("Show/hide main menu"));
    actShowHideMenuBar->setShortcut(QKeySequence::fromString(tr("ctrl+m")));
    actShowHideMenuBar->setCheckable(true);
    actShowHideMenuBar->setChecked(true);
    connect(actShowHideMenuBar, SIGNAL(triggered()), this, SLOT(action_ShowHideMenuBar()));
    INSERT_ACTION_MAP(actShowHideMenuBar);

    QAction *actShowHideFlightInfoPanel = new QAction(tr("FlightInfo Panel"), this);
    actShowHideFlightInfoPanel->setStatusTip(tr("Show/hide flight information panel"));
    actShowHideFlightInfoPanel->setCheckable(true);
    actShowHideFlightInfoPanel->setChecked(true);
    connect(actShowHideFlightInfoPanel, SIGNAL(triggered()), this, SLOT(action_ShowHideFlightInfoPanel()));
    INSERT_ACTION_MAP(actShowHideFlightInfoPanel);

    QAction *actShowMsgBoard = new QAction(tr("MsgBoard OSD"), this);
    actShowMsgBoard->setStatusTip(tr("Show/hide MsgBoard OSD"));
    //actShowMsgBoard->setShortcut(QKeySequence::fromString(tr("ctrl+o")));
    actShowMsgBoard->setCheckable(true);
    actShowMsgBoard->setChecked(true);
    connect(actShowMsgBoard, SIGNAL(triggered()), this, SLOT(action_ShowMsgBoard()));
    INSERT_ACTION_MAP(actShowMsgBoard);


    QAction *actShowStatusText = new QAction(tr("Status Text"), this);
    actShowStatusText->setStatusTip(tr("Show/Hide Status Text"));
    actShowStatusText->setCheckable(true);
    actShowStatusText->setChecked(svar.GetInt("FastGCS.ui.MapWidget.StatusText.show", 1));
    connect(actShowStatusText, SIGNAL(triggered()), this, SLOT(action_ShowStatusText()));
    INSERT_ACTION_MAP(actShowStatusText);


    QAction *actShowLogInfo = new QAction(tr("Log Information"), this);
    actShowLogInfo->setStatusTip(tr("Show/hide Log Information"));
    //actShowLogInfo->setShortcut(QKeySequence::fromString(tr("ctrl+o")));
    actShowLogInfo->setCheckable(true);
    actShowLogInfo->setChecked(svar.GetInt("FastGCS.ui.MapWidget.LogInfo.show", 1));
    connect(actShowLogInfo, SIGNAL(triggered()), this, SLOT(action_ShowLogInfo()));
    INSERT_ACTION_MAP(actShowLogInfo);

    QAction *actShowVideoFrame = new QAction(tr("Video Frame"), this);
    actShowVideoFrame->setStatusTip(tr("Show/Hide Video Frame"));
    actShowVideoFrame->setShortcut(QKeySequence::fromString(tr("ctrl+v")));
    actShowVideoFrame->setCheckable(true);
    actShowVideoFrame->setChecked(true);
    connect(actShowVideoFrame, SIGNAL(triggered()), this, SLOT(action_ShowVideoFrame()));
    INSERT_ACTION_MAP(actShowVideoFrame);

    QAction *actVideoFrameScaleUp = new QAction(tr("Video Scale Up"), this);
    actVideoFrameScaleUp->setStatusTip(tr("Video Scale Up"));
    actVideoFrameScaleUp->setShortcut(QKeySequence::fromString(tr("m")));
    connect(actVideoFrameScaleUp, SIGNAL(triggered()), this, SLOT(action_VideoFrameScaleUp()));
    INSERT_ACTION_MAP(actVideoFrameScaleUp);

    QAction *actVideoFrameScaleDown = new QAction(tr("Video Scale Down"), this);
    actVideoFrameScaleDown->setStatusTip(tr("Video Scale Down"));
    actVideoFrameScaleDown->setShortcut(QKeySequence::fromString(tr("n")));
    connect(actVideoFrameScaleDown, SIGNAL(triggered()), this, SLOT(action_VideoFrameScaleDown()));
    INSERT_ACTION_MAP(actVideoFrameScaleDown);


    QAction *actViewTab_Map = new QAction(tr("Show Map"), this);
    actViewTab_Map->setStatusTip(tr("Show Map Tab"));
    actViewTab_Map->setShortcut(QKeySequence::fromString(tr("ctrl+1")));
    connect(actViewTab_Map, SIGNAL(triggered()), this, SLOT(action_ViewTab_Map()));
    INSERT_ACTION_MAP(actViewTab_Map);

    QAction *actViewTab_SLAM = new QAction(tr("Show SLAM"), this);
    actViewTab_SLAM->setStatusTip(tr("Show SLAM"));
    actViewTab_SLAM->setShortcut(QKeySequence::fromString(tr("ctrl+2")));
    connect(actViewTab_SLAM, SIGNAL(triggered()), this, SLOT(action_ViewTab_SLAM()));
    INSERT_ACTION_MAP(actViewTab_SLAM);

    QAction *actShowHideGuidedTarget = new QAction(tr("Show/Hide guided target"), this);
    actShowHideGuidedTarget->setStatusTip(tr("Show/hide guided target"));
    connect(actShowHideGuidedTarget, SIGNAL(triggered()), this, SLOT(action_ShowHideGuidedTarget()));
    INSERT_ACTION_MAP(actShowHideGuidedTarget);

    QAction *actViewFastForward = new QAction(tr("Fast Forward"), this);
    actViewFastForward->setStatusTip(tr("FF recorded data"));
    actViewFastForward->setShortcut(QKeySequence::fromString(tr("ctrl+right")));
    connect(actViewFastForward, SIGNAL(triggered()), this, SLOT(action_ViewFastForward()));
    INSERT_ACTION_MAP(actViewFastForward);



    viewMenu->addAction(actShowFullScreen);
    viewMenu->addAction(actShowHideMenuBar);
    viewMenu->addAction(actShowHideStatusBar);
    viewMenu->addAction(actShowHideFlightInfoPanel);
    viewMenu->addSeparator();
    viewMenu->addAction(actViewTab_Map);
    viewMenu->addAction(actViewTab_SLAM);
    viewMenu->addSeparator();
    viewMenu->addAction(actShowMsgBoard);
    viewMenu->addAction(actShowStatusText);
    viewMenu->addAction(actShowLogInfo);
    viewMenu->addAction(actShowVideoFrame);
    viewMenu->addAction(actVideoFrameScaleDown);
    viewMenu->addAction(actVideoFrameScaleUp);
    viewMenu->addSeparator();
    viewMenu->addAction(m_mapView->getAction("actionGCS_ShowHide"));
    viewMenu->addAction(m_mapView->getAction("actionGCS_Safearea"));
    viewMenu->addAction(m_mapView->getAction("actionHome_ShowHide"));
    viewMenu->addAction(m_mapView->getAction("actionHome_Safearea"));
    viewMenu->addAction(actShowHideGuidedTarget);
    viewMenu->addSeparator();
    viewMenu->addAction(m_mapView->getAction("actionGotoHome"));
    viewMenu->addAction(m_mapView->getAction("actionGotoGCS"));
    viewMenu->addSeparator();
    viewMenu->addAction(actViewFastForward);


    /////////////////////////////////////////////
    /// MAV
    /////////////////////////////////////////////
    QMenu *mavMenu = new QMenu(tr("&MAV"));

    QAction *actClearPos = new QAction(tr("Clear UAVs and trails"), this);
    actClearPos->setStatusTip(tr("Clear UAVs and trails"));
    //actClearPos->setShortcut(QKeySequence::fromString(tr("ctrl+c")));
    connect(actClearPos, SIGNAL(triggered()), this, SLOT(action_ClearPos()));
    INSERT_ACTION_MAP(actClearPos);

    QAction *actLoadMAVParameters = new QAction(tr("Load Parameters"), this);
    actLoadMAVParameters->setStatusTip(tr("Load MAV/GCS parameter and display them"));
    actLoadMAVParameters->setShortcut(QKeySequence::fromString(tr("ctrl+p")));
    connect(actLoadMAVParameters, SIGNAL(triggered()), this, SLOT(action_loadMAVParameters()));
    INSERT_ACTION_MAP(actLoadMAVParameters);


    QAction *actUploadWaypoints = new QAction(tr("Waypoint Upload"), this);
    actUploadWaypoints->setStatusTip(tr("Upload waypoints to MAV"));
    actUploadWaypoints->setShortcut(QKeySequence(tr("Ctrl+u")));
    connect(actUploadWaypoints, SIGNAL(triggered()), this, SLOT(action_uploadWaypoints()));
    INSERT_ACTION_MAP(actUploadWaypoints);

    QAction *actDownloadWaypoints = new QAction(tr("Waypoint Download"), this);
    actDownloadWaypoints->setStatusTip(tr("Download waypoints from MAV"));
    actDownloadWaypoints->setShortcut(QKeySequence(tr("Ctrl+d")));
    connect(actDownloadWaypoints, SIGNAL(triggered()), this, SLOT(action_downloadWaypoints()));
    INSERT_ACTION_MAP(actDownloadWaypoints);

    QAction *actClearWaypoints = new QAction(tr("Waypoint Clear on MAV"), this);
    actClearWaypoints->setStatusTip(tr("Clear all WPs on MAV"));
    connect(actClearWaypoints, SIGNAL(triggered()), this, SLOT(action_clearWaypoints()));
    INSERT_ACTION_MAP(actClearWaypoints);

    QAction *actSetCurrentWP = new QAction(tr("Set Current Waypoint"), this);
    actSetCurrentWP->setStatusTip(tr("Set current waypoint"));
    connect(actSetCurrentWP, SIGNAL(triggered()), this, SLOT(action_setCurrentWP()));
    INSERT_ACTION_MAP(actSetCurrentWP);


    QAction *actPreflightCalibration = new QAction(tr("Prefilight Calibration"), this);
    actPreflightCalibration->setStatusTip(tr("Preflight Calibration"));
    connect(actPreflightCalibration, SIGNAL(triggered()), this, SLOT(action_preFlightCalibration()));
    INSERT_ACTION_MAP(actPreflightCalibration);

    QAction *actFlightModeConfig = new QAction(tr("FlightMode Configure"), this);
    actFlightModeConfig->setStatusTip(tr("Configure flight mode"));
    connect(actFlightModeConfig, SIGNAL(triggered()), this, SLOT(action_flightModeConfig()));
    INSERT_ACTION_MAP(actFlightModeConfig);

    QAction *actRCConfig = new QAction(tr("RC Configure"), this);
    actRCConfig->setStatusTip(tr("Configure or calibrate RC"));
    connect(actRCConfig, SIGNAL(triggered()), this, SLOT(action_rcConfig()));
    INSERT_ACTION_MAP(actRCConfig);

    QAction *actAccelConfig = new QAction(tr("Accelerator Configure"), this);
    actAccelConfig->setStatusTip(tr("Configure or calibrate accelerator"));
    connect(actAccelConfig, SIGNAL(triggered()), this, SLOT(action_accelConfig()));
    INSERT_ACTION_MAP(actAccelConfig);

    QAction *actCompassConfig = new QAction(tr("Compass Configure"), this);
    actCompassConfig->setStatusTip(tr("Configure or calibrate compass"));
    connect(actCompassConfig, SIGNAL(triggered()), this, SLOT(action_compassConfig()));
    INSERT_ACTION_MAP(actCompassConfig);

    QAction *actSetGimbalPitch = new QAction(tr("Set Gimbal Pitch"), this);
    actSetGimbalPitch->setStatusTip(tr("Set gimbal pitch PWM value"));
    connect(actSetGimbalPitch, SIGNAL(triggered()), this, SLOT(action_setGimbalPitch()));
    INSERT_ACTION_MAP(actSetGimbalPitch);

    QAction *actSetServo = new QAction(tr("Set Servo PWM"), this);
    actSetServo->setStatusTip(tr("Set Servo PWM value"));
    connect(actSetServo, SIGNAL(triggered()), this, SLOT(action_setServo()));
    INSERT_ACTION_MAP(actSetServo);

    QAction *actLandingGearDown = new QAction(tr("Landing Gear Down"), this);
    actLandingGearDown->setStatusTip(tr("Landing Gear Down"));
    connect(actLandingGearDown, SIGNAL(triggered()), this, SLOT(action_landingGearDown()));
    INSERT_ACTION_MAP(actLandingGearDown);

    QAction *actLandingGearUp = new QAction(tr("Landing Gear Up"), this);
    actLandingGearUp->setStatusTip(tr("Landing Gear Up"));
    connect(actLandingGearUp, SIGNAL(triggered()), this, SLOT(action_landingGearUp()));
    INSERT_ACTION_MAP(actLandingGearUp);


    QAction *actFollowMeStart = new QAction(tr("FollowMe Start"), this);
    actFollowMeStart->setStatusTip(tr("FollowMe start"));
    connect(actFollowMeStart, SIGNAL(triggered()), this, SLOT(action_followMeStart()));
    INSERT_ACTION_MAP(actFollowMeStart);

    QAction *actFollowMeStop = new QAction(tr("FollowMe Stop"), this);
    actFollowMeStop->setStatusTip(tr("FollowMe stop"));
    connect(actFollowMeStop, SIGNAL(triggered()), this, SLOT(action_followMeStop()));
    INSERT_ACTION_MAP(actFollowMeStop);

    QAction *actJoystickControlStart = new QAction(tr("Joystick Control Start"), this);
    actJoystickControlStart->setStatusTip(tr("Begin to use joystick"));
    connect(actJoystickControlStart, SIGNAL(triggered()), this, SLOT(action_joystickStart()));
    INSERT_ACTION_MAP(actJoystickControlStart);

    QAction *actJoystickControlStop = new QAction(tr("Joystick Control Stop"), this);
    actJoystickControlStop->setStatusTip(tr("Stop to use joystick"));
    connect(actJoystickControlStop, SIGNAL(triggered()), this, SLOT(action_joystickStop()));
    INSERT_ACTION_MAP(actJoystickControlStop);


    mavMenu->addAction(actClearPos);
    mavMenu->addAction(m_mapView->getAction("actionUAVTrail_clear"));
    mavMenu->addSeparator();
    mavMenu->addAction(actLoadMAVParameters);
    mavMenu->addSeparator();
    mavMenu->addAction(m_mapView->getAction("actionWaypoint_save"));
    mavMenu->addAction(m_mapView->getAction("actionWaypoint_load"));
    mavMenu->addAction(m_mapView->getAction("actionWaypoint_edit"));
    mavMenu->addAction(actUploadWaypoints);
    mavMenu->addAction(actDownloadWaypoints);
    mavMenu->addAction(actClearWaypoints);
    mavMenu->addAction(actSetCurrentWP);
    mavMenu->addSeparator();
    mavMenu->addAction(actPreflightCalibration);
    mavMenu->addAction(actFlightModeConfig);
    mavMenu->addAction(actRCConfig);
    mavMenu->addAction(actAccelConfig);
    mavMenu->addAction(actCompassConfig);
    mavMenu->addSeparator();
    mavMenu->addAction(actSetGimbalPitch);
    mavMenu->addAction(actSetServo);
    mavMenu->addAction(actLandingGearDown);
    mavMenu->addAction(actLandingGearUp);
    mavMenu->addSeparator();
    mavMenu->addAction(actFollowMeStart);
    mavMenu->addAction(actFollowMeStop);
    mavMenu->addAction(actJoystickControlStart);
    mavMenu->addAction(actJoystickControlStop);


    /////////////////////////////////////////////
    /// SLAM
    /////////////////////////////////////////////
    QMenu *slamMenu = new QMenu(tr("&SLAM"));

    QAction *actSLAM_Reset = new QAction(tr("Reset SLAM"), this);
    actSLAM_Reset->setStatusTip(tr("Rset SLAM system"));
    connect(actSLAM_Reset, SIGNAL(triggered()), this, SLOT(action_SLAM_Reset()));
    INSERT_ACTION_MAP(actSLAM_Reset);

    QAction *actSLAM_SaveKeyFrames = new QAction(tr("Save Keyframes"), this);
    actSLAM_SaveKeyFrames->setStatusTip(tr("Save Keyframes"));
    connect(actSLAM_SaveKeyFrames, SIGNAL(triggered()), this, SLOT(action_SLAM_SaveKeyFrames()));
    INSERT_ACTION_MAP(actSLAM_SaveKeyFrames);

    QAction *actSLAM_SaveMap2D = new QAction(tr("Save 2D Map"), this);
    actSLAM_SaveMap2D->setStatusTip(tr("save fused 2D map"));
    connect(actSLAM_SaveMap2D, SIGNAL(triggered()), this, SLOT(action_SLAM_SaveMap2D()));
    INSERT_ACTION_MAP(actSLAM_SaveMap2D);

    QAction *actSLAM_ViewNormal = new QAction(tr("SLAM.ViewNormal"), this);
    actSLAM_ViewNormal->setStatusTip(tr("SLAM Normal View"));
    actSLAM_ViewNormal->setShortcut(QKeySequence::fromString(tr("alt+1")));
    connect(actSLAM_ViewNormal, SIGNAL(triggered()), this, SLOT(action_SLAM_ViewNormal()));
    INSERT_ACTION_MAP(actSLAM_ViewNormal);

    QAction *actSLAM_ViewSimple = new QAction(tr("SLAM.ViewSimple"), this);
    actSLAM_ViewSimple->setStatusTip(tr("SLAM Simple View"));
    actSLAM_ViewSimple->setShortcut(QKeySequence::fromString(tr("alt+2")));
    connect(actSLAM_ViewSimple, SIGNAL(triggered()), this, SLOT(action_SLAM_ViewSimple()));
    INSERT_ACTION_MAP(actSLAM_ViewSimple);

    QAction *actSLAM_ViewFull = new QAction(tr("SLAM.ViewFull"), this);
    actSLAM_ViewFull->setStatusTip(tr("SLAM Full View"));
    actSLAM_ViewFull->setShortcut(QKeySequence::fromString(tr("alt+3")));
    connect(actSLAM_ViewFull, SIGNAL(triggered()), this, SLOT(action_SLAM_ViewFull()));
    INSERT_ACTION_MAP(actSLAM_ViewFull);

    QAction *actSLAM_ViewFineMap = new QAction(tr("SLAM.ViewFineMap"), this);
    actSLAM_ViewFineMap->setStatusTip(tr("SLAM Full View"));
    actSLAM_ViewFineMap->setShortcut(QKeySequence::fromString(tr("alt+4")));
    connect(actSLAM_ViewFineMap, SIGNAL(triggered()), this, SLOT(action_SLAM_ViewFineMap()));
    INSERT_ACTION_MAP(actSLAM_ViewFineMap);

    QAction *actSLAM_FollowCamera = new QAction(tr("SLAM.FollowCamera"), this);
    actSLAM_FollowCamera->setStatusTip(tr("SLAM Follow Camera View"));
    actSLAM_FollowCamera->setShortcut(QKeySequence::fromString(tr("alt+0")));
    connect(actSLAM_FollowCamera, SIGNAL(triggered()), this, SLOT(action_SLAM_FollowCamera()));
    INSERT_ACTION_MAP(actSLAM_FollowCamera);


    QAction *actSLAM_FitGPS = new QAction(tr("SLAM.FitGPS"), this);
    actSLAM_FitGPS->setStatusTip(tr("Fit GPS"));
    actSLAM_FitGPS->setShortcut(QKeySequence::fromString(tr("ctrl+f")));
    connect(actSLAM_FitGPS, SIGNAL(triggered()), this, SLOT(action_SLAM_FitGPS()));
    INSERT_ACTION_MAP(actSLAM_FitGPS);

    slamMenu->addAction(actSLAM_Reset);
    slamMenu->addAction(actSLAM_SaveKeyFrames);
    slamMenu->addAction(actSLAM_SaveMap2D);
    slamMenu->addSeparator();
    slamMenu->addAction(actSLAM_ViewNormal);
    slamMenu->addAction(actSLAM_ViewSimple);
    slamMenu->addAction(actSLAM_ViewFull);
    slamMenu->addAction(actSLAM_ViewFineMap);
    slamMenu->addAction(actSLAM_FollowCamera);
    slamMenu->addSeparator();
    slamMenu->addAction(actSLAM_FitGPS);


    /////////////////////////////////////////////
    /// GCS
    /////////////////////////////////////////////
    QMenu *gcsMenu = new QMenu(tr("&GCS"));

    QAction *actGCS_setGCS = new QAction(tr("Set GCS Positon"), this);
    connect(actGCS_setGCS, SIGNAL(triggered()), this, SLOT(action_GCS_setGCS()));
    INSERT_ACTION_MAP(actGCS_setGCS);

    QAction *actGCS_TestATA = new QAction(tr("Test ATA"), this);
    actGCS_TestATA->setShortcut(QKeySequence(tr("Ctrl+t")));
    connect(actGCS_TestATA, SIGNAL(triggered()), this, SLOT(action_GCS_testATA()));
    INSERT_ACTION_MAP(actGCS_TestATA);

    QAction *actGCS_SetYawOffset = new QAction(tr("Set Yaw Offset"), this);
    //actGCS_SetYawOffset->setShortcut(QKeySequence(tr("Ctrl+y")));
    connect(actGCS_SetYawOffset, SIGNAL(triggered()), this, SLOT(action_GCS_setYawOffset()));
    INSERT_ACTION_MAP(actGCS_SetYawOffset);


    QAction *actGCS_rcON = new QAction(tr("RC to MAV [ON]"), this);
    connect(actGCS_rcON, SIGNAL(triggered()), this, SLOT(action_GCS_rcON()));
    INSERT_ACTION_MAP(actGCS_rcON);

    QAction *actGCS_rcOFF = new QAction(tr("RC to MAV [OFF]"), this);
    connect(actGCS_rcOFF, SIGNAL(triggered()), this, SLOT(action_GCS_rcOFF()));
    INSERT_ACTION_MAP(actGCS_rcOFF);

    gcsMenu->addAction(actGCS_setGCS);
    gcsMenu->addAction(actGCS_SetYawOffset);
    gcsMenu->addAction(actGCS_TestATA);
    gcsMenu->addSeparator();
    gcsMenu->addAction(actGCS_rcON);
    gcsMenu->addAction(actGCS_rcOFF);


    /////////////////////////////////////////////
    /// Test
    /////////////////////////////////////////////
    QMenu *testMenu = new QMenu(tr("&Test"));

    QAction *actTest_SaveVideo = new QAction(tr("Save Video"), this);
    actTest_SaveVideo->setShortcut(QKeySequence(tr("Alt+c")));
    connect(actTest_SaveVideo, SIGNAL(triggered()), this, SLOT(action_Test_saveVideo()));
    INSERT_ACTION_MAP(actTest_SaveVideo);

    QAction *actTest_SendMission = new QAction(tr("Send Mission to VUAV"), this);
    connect(actTest_SendMission, SIGNAL(triggered()), this, SLOT(action_Test_sendMission()));
    INSERT_ACTION_MAP(actTest_SendMission);

    QAction *actTest_StartMission = new QAction(tr("Start Mission"), this);
    connect(actTest_StartMission, SIGNAL(triggered()), this, SLOT(action_Test_startMission()));
    INSERT_ACTION_MAP(actTest_StartMission);

    QAction *actTest_StopMission = new QAction(tr("Stop Mission"), this);
    connect(actTest_StopMission, SIGNAL(triggered()), this, SLOT(action_Test_stopMission()));
    INSERT_ACTION_MAP(actTest_StopMission);

    QAction *actTest_ExeCommand = new QAction(tr("Exec Command"), this);
    connect(actTest_ExeCommand, SIGNAL(triggered()), this, SLOT(action_Test_execCommand()));
    INSERT_ACTION_MAP(actTest_ExeCommand);

    testMenu->addAction(actTest_SaveVideo);
    testMenu->addSeparator();
    testMenu->addAction(actTest_SendMission);
    testMenu->addAction(actTest_StartMission);
    testMenu->addAction(actTest_StopMission);
    testMenu->addSeparator();
    testMenu->addAction(actTest_ExeCommand);


    /////////////////////////////////////////////
    /// Help
    /////////////////////////////////////////////
    QMenu *helpMenu = new QMenu(tr("&Help"));

    QAction *actHelpt_about = new QAction(tr("About ..."), this);
    connect(actHelpt_about, SIGNAL(triggered()), this, SLOT(action_Help_about()));
    INSERT_ACTION_MAP(actHelpt_about);

    helpMenu->addAction(actHelpt_about);



    /////////////////////////////////////////////
    /// add all actions to main window
    /////////////////////////////////////////////
    foreach(QAction* action, m_actionMap) {
        //add to MainWindow so they work when menu is hidden
        addAction(action);
    }

    // add to main menu
    #define INSERT_MENU_ITEM(x) { menuBar()->addMenu(x); m_menuList.push_back(x); }

    INSERT_MENU_ITEM(fileMenu);
    INSERT_MENU_ITEM(editMenu);
    INSERT_MENU_ITEM(viewMenu);
    INSERT_MENU_ITEM(mavMenu);
    INSERT_MENU_ITEM(slamMenu);
    INSERT_MENU_ITEM(gcsMenu);
    INSERT_MENU_ITEM(testMenu);
    INSERT_MENU_ITEM(helpMenu);

    // map widget add main menu
    m_mapView->setupMainMenu(m_menuList);

    // MapWidget add waypoints menu
    m_mapView->setupExMenu(m_actionMap);


    // hide main menu according setting
    if( svar.GetInt("FastGCS.ui.mainMenu.show", 0) )
        action_ShowHideMenuBar();
}



void GCS_MainWindow::createMainMenu(void)
{
    return;
}

void GCS_MainWindow::createToolBars(void)
{
    menuToolBar = addToolBar(tr("ToolbarMain"));
    menuToolBar->setObjectName("ToolbarMain");
    menuToolBar->setMovable(true);

    //addToolBarBreak();

    QLineEdit *editAddress = new QLineEdit(this);
    menuToolBar->addWidget(editAddress);
}


QAction* GCS_MainWindow::getAction(const QString &actName)
{
    QMap<QString, QAction*>::iterator iter;

    iter = m_actionMap.find(actName);
    if( iter == m_actionMap.end() ) {
        return NULL;
    } else {
        return iter.value();
    }
}


void GCS_MainWindow::action_Open(void)
{
    UI_diagComOpen  diag;
    UI_diagComOpen::ComDev_Type t;
    QString v1, v2;

    if( !diag.exec() ) return;

    // get device info
    diag.getDevInfo(t, v1, v2);

    // close old connection & reset all
    if( m_dataManager == NULL ) return;
    m_dataManager->close();

    // reset view widgets
    m_mapView->reset();
    if( m_plot2D != NULL ) m_plot2D->clearPoint();
    if( m_plot3D != NULL ) m_plot3D->clearPoint();

    // open device
    if( t == UI_diagComOpen::COMDEV_UART ) {
        m_dataManager->open(DataManager::DATA_LIVE, v1.toStdString(), v2.toInt());
    } else if( t == UI_diagComOpen::COMDEV_FILE ) {
        m_dataManager->open(DataManager::DATA_FILE, v1.toStdString(), v2.toInt());
    } else if( t == UI_diagComOpen::COMDEV_UDP ) {
        m_dataManager->open(DataManager::DATA_UDP, v1.toStdString(), v2.toInt());
    } else if( t == UI_diagComOpen::COMDEV_VIRTUAL_UAV ) {
        m_dataManager->open(DataManager::DATA_SIMULATION , v1.toStdString(), v2.toInt());

        VirtualUAV_Manager *vuav_manager = m_dataManager->getVUAVManager();
        if( vuav_manager == NULL ) return;

        // get home position
        internals::PointLatLng homePos;
        double homeAlt;
        m_mapView->getHome(homePos, homeAlt);

        // create and set VirtualUAV
        VirtualUAV *vuav;

        if( v1 == "QuadCopter" ) {
            vuav = new VirtualUAV_Quad();
            vuav->uavType = MAV_TYPE_QUADROTOR;
        } else if( v1 == "FixWings" ) {
            vuav = new VirtualUAV_Fixedwing();
            vuav->uavType = MAV_TYPE_FIXED_WING;
        }

        if( vuav == NULL ) return;

        vuav->ID = v2.toInt();
        vuav->initUAV(homePos.Lat(), homePos.Lng(), homeAlt, 0);
        vuav->updateTime(tm_getTimeStamp());

        vuav_manager->addUAV(vuav);
    }
}

void GCS_MainWindow::action_Close(void)
{
    // confirm quit
    QMessageBox msgbox;
    msgbox.setWindowTitle("Confirm");
    msgbox.setText("Are you sure to close communication?");
    msgbox.setStandardButtons( QMessageBox::Cancel | QMessageBox::Ok );
    msgbox.setDefaultButton(QMessageBox::Cancel);

    if( msgbox.exec() == QMessageBox::Ok ) {
        if( m_dataManager != NULL ) m_dataManager->close();

        // reset view widgets
        m_mapView->reset();
        m_uasManager->reset();
        if( m_plot3D != NULL ) m_plot2D->clearPoint();
        if( m_plot3D != NULL ) m_plot3D->clearPoint();
    }
}

void GCS_MainWindow::action_Quit(void)
{
    // confirm quit
    QMessageBox msgbox;
    msgbox.setWindowTitle("Confirm");
    msgbox.setText("Are you sure to quit?");
    msgbox.setStandardButtons( QMessageBox::Cancel | QMessageBox::Ok );
    msgbox.setDefaultButton(QMessageBox::Cancel);
    msgbox.show();
    msgbox.move(geometry().center().x()-msgbox.size().width()/2,
                geometry().center().y()-msgbox.size().height()/2);

    if( msgbox.exec() == QMessageBox::Ok ) {
        // close SvarEdit
        if( SvarWithType<SvarWidget*>::instance().exist("SvarWidget") ) {
            SvarWidget *sw = SvarWithType<SvarWidget*>::instance()["SvarWidget"];
            sw->close();
        }

        qApp->exit(0);
    }
}

void GCS_MainWindow::action_SvarEdit(void)
{
    SvarWidget *sw;

    if( SvarWithType<SvarWidget*>::instance().exist("SvarWidget") ) {
        sw = SvarWithType<SvarWidget*>::instance()["SvarWidget"];
    } else {
        sw = new SvarWidget();
        SvarWithType<SvarWidget*>::instance()["SvarWidget"] = sw;
    }

    sw->show();
}

void GCS_MainWindow::action_ShowFullScreen(void)
{
    if( m_actionMap["actShowFullScreen"]->isChecked() ) {
        showFullScreen();
    } else {
        showNormal();
    }
}

void GCS_MainWindow::action_ShowHideStatusBar(void)
{
    if( statusBar()->isHidden() )
        statusBar()->setHidden(false);
    else
        statusBar()->setHidden(true);
}

void GCS_MainWindow::action_ShowHideMenuBar(void)
{
    if( menuBar()->isHidden() ) {
        menuBar()->setHidden(false);
        m_actionMap["actShowHideMenuBar"]->setChecked(true);
    } else {
        menuBar()->setHidden(true);
        m_actionMap["actShowHideMenuBar"]->setChecked(false);
    }
}

void GCS_MainWindow::action_ShowHideFlightInfoPanel(void)
{
    if( m_flightInfoPanel->isHidden() ) {
        m_flightInfoPanel->show();
        m_actionMap["actShowHideFlightInfoPanel"]->setChecked(true);
    } else {
        m_flightInfoPanel->hide();
        m_actionMap["actShowHideFlightInfoPanel"]->setChecked(false);
    }
}

void GCS_MainWindow::action_ShowMsgBoard(void)
{
    if( m_mapView->getShowMsgBoard() ) {
        m_mapView->setShowMsgBoard(false);
        m_actionMap["actShowMsgBoard"]->setChecked(false);
    } else {
        m_mapView->setShowMsgBoard(true);
        m_actionMap["actShowMsgBoard"]->setChecked(true);
    }
}

void GCS_MainWindow::action_ShowStatusText(void)
{
    int &v = svar.GetInt("FastGCS.ui.MapWidget.StatusText.show", 1);

    if( v ) {
        v = 0;
        m_actionMap["actShowStatusText"]->setChecked(false);
        //m_mapView->ReloadMap();
    } else {
        v = 1;
        m_actionMap["actShowStatusText"]->setChecked(true);
    }
}


void GCS_MainWindow::action_ShowLogInfo(void)
{
    if( m_mapView->getShowLogInfo() ) {
        m_mapView->setShowLogInfo(false);
        m_actionMap["actShowLogInfo"]->setChecked(false);
    } else {
        m_mapView->setShowLogInfo(true);
        m_actionMap["actShowLogInfo"]->setChecked(true);
    }
}

void GCS_MainWindow::action_ShowVideoFrame(void)
{
    if( m_mapView->getShowVideoFrame() ) {
        m_mapView->setShowVideoFrame(false);
        m_actionMap["actShowVideoFrame"]->setChecked(false);
    } else {
        m_mapView->setShowVideoFrame(true);
        m_actionMap["actShowVideoFrame"]->setChecked(true);
    }
}

void GCS_MainWindow::action_VideoFrameScaleUp(void)
{
    double& scale=svar.d["FrameTracker.ShowFrameScale"];
    scale *= 1.2;

    m_mapView->setVideoFrameScale(scale);
}

void GCS_MainWindow::action_VideoFrameScaleDown(void)
{
    double& scale=svar.d["FrameTracker.ShowFrameScale"];
    scale /= 1.2;

    m_mapView->setVideoFrameScale(scale);
}

void GCS_MainWindow::action_PlaySpeed(void)
{
    double &playSpeed = *SvarDouble("FastGCS.PlaySpeed", 1.0);

    // get user input
    bool ok = true;
    double ps = QInputDialog::getDouble(this,
                                         tr("Play speed"),
                                         tr("Speed (0.1~10x):"),
                                         playSpeed, 0.1, 10,
                                         1,
                                         &ok);
    if ( ok ) playSpeed = ps;
}


void GCS_MainWindow::action_ViewTab_Map(void)
{
    m_tabWidget->setCurrentIndex(0);
}

void GCS_MainWindow::action_ViewTab_SLAM(void)
{
    m_tabWidget->setCurrentIndex(1);
}


void GCS_MainWindow::action_ShowHideGuidedTarget(void)
{
    bool s = m_mapView->ShowGuidedTarget();
    m_mapView->SetShowGuidedTarget(!s);
}

void GCS_MainWindow::action_ViewFastForward(void)
{
    if( m_dataManager != NULL )
        m_dataManager->seek(svar.GetDouble("FastGCS.View.FF", 30));
}


void GCS_MainWindow::action_ClearPos(void)
{
    // confirm clear
    QMessageBox msgbox;
    msgbox.setWindowTitle("Confirm");
    msgbox.setText("Are you sure to clear?");
    msgbox.setStandardButtons( QMessageBox::Cancel | QMessageBox::Ok );
    msgbox.setDefaultButton(QMessageBox::Cancel);

    if( msgbox.exec() != QMessageBox::Ok ) return;


    if( m_plot2D != NULL ) m_plot2D->clearPoint();
    if( m_plot3D != NULL ) m_plot3D->clearPoint();

    if( m_uasManager != NULL ) {
        UAS_Base *u = m_uasManager->get_active_mav();
        if( u != NULL ) u->clear_home();
    }

    // FIXME:
    mapcontrol::UAVItem *u = m_mapView->GetUAV(0);
    if( u != NULL  ) {
        m_mapView->DeleteUAV(0);
    }
}

void GCS_MainWindow::action_loadMAVParameters(void)
{
    if( m_uasManager == NULL ) return;

    // load all parameters from UAS
    if( 1 ) {
        UAS_Base  *u;
        UAS_Array *arrUAS;
        UAS_Array::iterator it;

        arrUAS = m_uasManager->get_uas();
        for(it=arrUAS->begin(); it!=arrUAS->end(); it++) {
            u = *it;

            if( u->uasType == UAS_TYPE_TELEM ) continue;

            if( u->m_paramArray.isLoaded() ) continue;

            // load parameters from UAS
            u->requireParameters();

            // show progress
            UI_diagParamProgress dlg;
            dlg.setUAS(u);
            dlg.exec();
        }
    }

    // show parameter window
    if( 1 ) {
        UI_AP_paramControl *pcForm = new UI_AP_paramControl(NULL);
        pcForm->set_UAS_Manager(m_uasManager);
        pcForm->setWindowTitle("MAV Parameters");
        pcForm->show();
        pcForm->resize(800, 600);
    }
}


void GCS_MainWindow::action_uploadWaypoints(void)
{
    if( m_uasManager == NULL ) return;

    UAS_Base *u = m_uasManager->get_active_mav();
    if( u == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();

        return;
    }

    // get current waypoints & upload to MAV
    AP_MissionArray wpa;
    m_mapView->getWaypoints(wpa);

    if( wpa.size() > 0 ) {
        u->writeWaypoints(wpa);

        UI_diagMissionProgress diag;
        diag.setUAS(u, 1);
        diag.exec();
    } else {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No effective waypoints!");
        msgbox.exec();
    }
}

void GCS_MainWindow::action_downloadWaypoints(void)
{
    if( m_uasManager == NULL ) return;

    UAS_Base *u = m_uasManager->get_active_mav();
    if( u == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();

        return;
    }

    u->readWaypoints();

    UI_diagMissionProgress diag;
    diag.setUAS(u, 0);
    diag.exec();

    // set loaded waypoints to map widget
    m_mapView->setWaypoints(u->m_waypoints);
}

void GCS_MainWindow::action_clearWaypoints(void)
{
    // clear map view's waypoints
    //m_mapView->actWaypoint_clear();

    // clear UAV's waypoints
    if( m_uasManager == NULL ) return;

    UAS_Base *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    printf("Clear all waypoints of MAV[%3d]\n", u->ID);
    u->clearWaypoints();
}

void GCS_MainWindow::action_setCurrentWP(void)
{
    if( m_uasManager == NULL ) return;

    UAS_Base *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    // get home altitude
    bool ok = true;
    int idxWP = QInputDialog::getInt(this,
                                     tr("Input current waypoint"),
                                     tr("Waypoint index:"),
                                     1, 0, 10000,
                                     1,
                                     &ok);
    if ( !ok ) return;

    printf("Set current waypoint to: %d\n", idxWP);
    u->setCurrentWaypoint(idxWP);
}


void GCS_MainWindow::action_preFlightCalibration(void)
{
    if( m_uasManager == NULL ) return;

    UAS_Base *u = m_uasManager->get_active_mav();
    if( u == NULL ) {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No active MAV exist!");
        msgbox.exec();
        return;
    }

    // send gyro calibration command
    {
        MAV_CMD command = MAV_CMD_PREFLIGHT_CALIBRATION;
        int confirm = 0;
        float param1 = 1.0;
        float param2 = 0.0;
        float param3 = 0.0;
        float param4 = 0.0;
        float param5 = 0.0;
        float param6 = 0.0;
        float param7 = 0.0;
        int component = 1;

        u->executeCommand(command, confirm,
                              param1, param2, param3, param4, param5, param6, param7,
                              component);
        dbg_pi("Init gyro");

        tm_sleep(50);
    }

    // send barometer calibration command
    {
        MAV_CMD command = MAV_CMD_PREFLIGHT_CALIBRATION;
        int confirm = 0;
        float param1 = 0.0;
        float param2 = 0.0;
        float param3 = 1.0;
        float param4 = 0.0;
        float param5 = 0.0;
        float param6 = 0.0;
        float param7 = 0.0;
        int component = 1;

        u->executeCommand(command, confirm,
                              param1, param2, param3, param4, param5, param6, param7,
                              component);

        dbg_pi("Init barometer");

        tm_sleep(50);
    }
}

void GCS_MainWindow::action_flightModeConfig(void)
{
    if( m_uasManager == NULL ) return;

    // get selected UAV
    UI_diagUAVSelect diagSelectUAV(this, m_uasManager);
    if( !diagSelectUAV.exec() ) return;

    int uasID = diagSelectUAV.getSelectedUAV();

    if( SvarWithType<UI_flightModeConfig*>::instance().exist("FlightModeControl.ptr") ) {
        UI_flightModeConfig *ccWidget = SvarWithType<UI_flightModeConfig*>::instance()["FlightModeControl.ptr"];

        if( !ccWidget->isActiveWindow() ) {
            delete ccWidget;
            ccWidget = new UI_flightModeConfig(NULL);
            SvarWithType<UI_flightModeConfig*>::instance()["FlightModeControl.ptr"] = ccWidget;
        }

        ccWidget->setUAS(m_uasManager->get_uas(uasID));
        ccWidget->show();
    } else {
        UI_flightModeConfig *ccWidget = new UI_flightModeConfig(NULL);
        SvarWithType<UI_flightModeConfig*>::instance()["FlightModeControl.ptr"] = ccWidget;
        ccWidget->setUAS(m_uasManager->get_uas(uasID));
        ccWidget->show();
    }
}

void GCS_MainWindow::action_rcConfig(void)
{
    if( m_uasManager == NULL ) return;

    // get selected UAV
    UI_diagUAVSelect diagSelectUAV(this, m_uasManager);
    if( !diagSelectUAV.exec() ) return;

    int uasID = diagSelectUAV.getSelectedUAV();

    if( SvarWithType<UI_rcConfigControl*>::instance().exist("RCConfigControl.ptr") ) {
        UI_rcConfigControl *ccWidget = SvarWithType<UI_rcConfigControl*>::instance()["RCConfigControl.ptr"];

        if( !ccWidget->isActiveWindow() ) {
            delete ccWidget;
            ccWidget = new UI_rcConfigControl(NULL);
            SvarWithType<UI_rcConfigControl*>::instance()["RCConfigControl.ptr"] = ccWidget;
        }

        ccWidget->setUAS(m_uasManager->get_uas(uasID));
        ccWidget->show();
    } else {
        UI_rcConfigControl *ccWidget = new UI_rcConfigControl(NULL);
        SvarWithType<UI_rcConfigControl*>::instance()["RCConfigControl.ptr"] = ccWidget;
        ccWidget->setUAS(m_uasManager->get_uas(uasID));
        ccWidget->show();
    }
}

void GCS_MainWindow::action_accelConfig(void)
{
    if( m_uasManager == NULL ) return;

    // get selected UAV
    UI_diagUAVSelect diagSelectUAV(this, m_uasManager);
    if( !diagSelectUAV.exec() ) return;

    int uasID = diagSelectUAV.getSelectedUAV();

    if( SvarWithType<UI_diagAccelConfig*>::instance().exist("AcceleratorConfigControl.ptr") ) {
        UI_diagAccelConfig *ccWidget = SvarWithType<UI_diagAccelConfig*>::instance()["AcceleratorConfigControl.ptr"];

        if( !ccWidget->isActiveWindow() ) {
            delete ccWidget;
            ccWidget = new UI_diagAccelConfig(NULL);
            SvarWithType<UI_diagAccelConfig*>::instance()["AcceleratorConfigControl.ptr"] = ccWidget;
        }

        ccWidget->setUAS(m_uasManager->get_uas(uasID));
        ccWidget->show();
    } else {
        UI_diagAccelConfig *ccWidget = new UI_diagAccelConfig(NULL);
        SvarWithType<UI_diagAccelConfig*>::instance()["AcceleratorConfigControl.ptr"] = ccWidget;
        ccWidget->setUAS(m_uasManager->get_uas(uasID));
        ccWidget->show();
    }
}

void GCS_MainWindow::action_compassConfig(void)
{
    if( m_uasManager == NULL ) return;

    // get selected UAV
    UI_diagUAVSelect diagSelectUAV(this, m_uasManager);
    if( !diagSelectUAV.exec() ) return;

    int uasID = diagSelectUAV.getSelectedUAV();

    if( SvarWithType<UI_compassConfigControl*>::instance().exist("CompassConfigControl.ptr") ) {
        UI_compassConfigControl *ccWidget = SvarWithType<UI_compassConfigControl*>::instance()["CompassConfigControl.ptr"];

        if( !ccWidget->isActiveWindow() ) {
            delete ccWidget;
            ccWidget = new UI_compassConfigControl(NULL);
            SvarWithType<UI_compassConfigControl*>::instance()["CompassConfigControl.ptr"] = ccWidget;
        }

        ccWidget->setUAS(m_uasManager->get_uas(uasID));
        ccWidget->show();
    } else {
        UI_compassConfigControl *ccWidget = new UI_compassConfigControl(NULL);
        SvarWithType<UI_compassConfigControl*>::instance()["CompassConfigControl.ptr"] = ccWidget;
        ccWidget->setUAS(m_uasManager->get_uas(uasID));
        ccWidget->show();
    }
}


void GCS_MainWindow::action_setGimbalPitch(void)
{
    if( m_uasManager == NULL ) return;

    UAS_Base *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    // get gimbal channel
    int &gimbalPitchChannel = svar.GetInt("Mavlink.Gimbal_Pitch_Channel", 10);
    int &gimbalPitchPWM     = svar.GetInt("Mavlink.Gimbal_Pitch_PWM", 1500);

    // get user input PWM value
    bool ok = true;
    gimbalPitchPWM = QInputDialog::getInt(this,
                                   tr("Input Gimble Pitch PWM value"),
                                   tr("PWM:"),
                                   gimbalPitchPWM,          // default value
                                   982, 2006,               // min/max
                                   1,                       // step
                                   &ok);
    if ( !ok ) return;

    dbg_pt("Set Gimbal Pitch to: %d (CH: %d)\n",
           gimbalPitchPWM, gimbalPitchChannel);

    // FIXME: send several times
    for(int i=0; i<3; i++) {
        u->_MAV_CMD_DO_SET_SERVO(gimbalPitchChannel, gimbalPitchPWM);
        tm_sleep(10);
    }
}

void GCS_MainWindow::action_setServo(void)
{
    if( m_uasManager == NULL ) return;

    UAS_Base *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    // get user input PWM value
    bool ok = true;
    int channel = QInputDialog::getInt(this,
                                       tr("Input Servo Channel"),
                                       tr("Channel:"),
                                       9,                       // default value
                                       1, 14,                   // min/max
                                       1,                       // step
                                       &ok);
    if( !ok ) return;

    ok = true;
    int pwm = QInputDialog::getInt(this,
                                   tr("Input PWM value"),
                                   tr("PWM:"),
                                   1000,                    // default value
                                   0, 10000,                // min/max
                                   1,                       // step
                                   &ok);
    if ( !ok ) return;

    dbg_pt("Set CH%d servor to: %d\n", channel, pwm);
    for(int i=0; i<3; i++) {
        u->_MAV_CMD_DO_SET_SERVO(channel, pwm);
        tm_sleep(10);
    }
}


void GCS_MainWindow::action_landingGearDown(void)
{
    if( m_uasManager == NULL ) return;

    UAS_MAV *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    u->landingGearDown();
}

void GCS_MainWindow::action_landingGearUp(void)
{
    if( m_uasManager == NULL ) return;

    UAS_MAV *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    u->landingGearUp();
}


void GCS_MainWindow::action_followMeStart(void)
{
    m_flightControl->btnFollowMeStart_clicked();
}

void GCS_MainWindow::action_followMeStop(void)
{
    m_flightControl->btnFollowMeStop_clicked();
}

void GCS_MainWindow::action_joystickStart(void)
{
    // get active MAV
    if( m_uasManager == NULL ) return;
    UAS_MAV *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    // open joystick & detect joystic is open or not
    pi::HAL_JSComobo* js = m_dataManager->getJoystick();
    js->open(svar.GetString("FastGCS.joystick", "Joystick.Madcatz_X-55"));

    if( !js->isOpened() ) {
        dbg_pe("Joystick device is not opened!");
        return;
    }

    // check current status
    //FIXME: should be guided, yet px4 does not support such mode
    if( u->customMode != MFM_STABILIZED ) {
        dbg_pe("Joystick control must be in OFFBOARD mode");
        return;
    }

    // start guided control
    u->m_guidedControl.turnOn();
    u->m_guidedControl.controlSource = UAS_MAV::GCS_JOYSTICK;
    u->m_guidedControl.tmLastAction = 0;
}

void GCS_MainWindow::action_joystickStop(void)
{
    // get active MAV
    if( m_uasManager == NULL ) return;
    UAS_MAV *u = m_uasManager->get_active_mav();
    if( u == NULL ) return;

    // start guided control
    u->m_guidedControl.controlSource = UAS_MAV::GCS_MOUSE;
    u->m_guidedControl.turnOff();

    // close joystick
    pi::HAL_JSComobo* js = m_dataManager->getJoystick();
    js->close();
}


void GCS_MainWindow::action_SLAM_Reset(void)
{

}

void GCS_MainWindow::action_SLAM_SaveKeyFrames(void)
{

}

void GCS_MainWindow::action_SLAM_SaveMap2D(void)
{
    if( m_dataManager == NULL ) return;

    // get save filename
    QString fname, path = ".";

    fname = QFileDialog::getSaveFileName(this, tr("Map data"),
                                         path,
                                         tr("Map image (*.png)"));
    if( fname.size() < 1 ) return;

    // save map
    string cmd = fmt::sprintf("Map2DSave %s", fname.toStdString());
    svar.ParseLine(cmd);
}


void GCS_MainWindow::action_SLAM_ViewNormal(void)
{
    svar.GetInt("Win3D.DrawInfoText", 1)                = 1;
    svar.GetInt("FrameTracker.DrawCurrentConnects", 1)  = 0;
    svar.GetInt("FrameTracker.DrawMatchedPoints", 1)    = 1;
    svar.GetInt("Map.DrawDataArea", 1)                  = 0;
    svar.GetInt("Map.DrawGPSPath", 1)                   = 1;
    svar.GetInt("Map.DrawKFRect", 1)                    = 1;
    svar.GetInt("Map.DrawLocalPoints", 1)               = 1;
    svar.GetInt("Map.DrawPoints", 1)                    = 2;
    svar.GetInt("Map.DrawKeyframes", 1)                 = 0;
    svar.GetInt("VideoFrame.DrawTexture", 1)            = 0;

    svar.GetInt("Map2D.drawResult", 0)                  = 0;
}

void GCS_MainWindow::action_SLAM_ViewSimple(void)
{
    svar.GetInt("Win3D.DrawInfoText", 1)                = 1;
    svar.GetInt("FrameTracker.DrawCurrentConnects", 1)  = 0;
    svar.GetInt("FrameTracker.DrawMatchedPoints", 1)    = 0;
    svar.GetInt("Map.DrawDataArea", 1)                  = 0;
    svar.GetInt("Map.DrawGPSPath", 1)                   = 0;
    svar.GetInt("Map.DrawKFRect", 1)                    = 0;
    svar.GetInt("Map.DrawLocalPoints", 1)               = 0;
    svar.GetInt("Map.DrawPoints", 1)                    = 0;
    svar.GetInt("Map.DrawKeyframes", 1)                 = 0;
    svar.GetInt("VideoFrame.DrawTexture", 1)            = 1;

    svar.GetInt("Map2D.drawResult", 0)                  = 0;
}

void GCS_MainWindow::action_SLAM_ViewFull(void)
{
    svar.GetInt("Win3D.DrawInfoText", 1)                = 1;
    svar.GetInt("FrameTracker.DrawCurrentConnects", 1)  = 1;
    svar.GetInt("FrameTracker.DrawMatchedPoints", 1)    = 1;
    svar.GetInt("Map.DrawDataArea", 1)                  = 0;
    svar.GetInt("Map.DrawGPSPath", 1)                   = 1;
    svar.GetInt("Map.DrawKFRect", 1)                    = 1;
    svar.GetInt("Map.DrawLocalPoints", 1)               = 1;
    svar.GetInt("Map.DrawPoints", 1)                    = 2;
    svar.GetInt("Map.DrawKeyframes", 1)                 = 1;
    svar.GetInt("VideoFrame.DrawTexture", 1)            = 1;

    svar.GetInt("Map2D.drawResult", 0)                  = 0;
}

void GCS_MainWindow::action_SLAM_ViewFineMap(void)
{
    svar.GetInt("Win3D.DrawInfoText", 1)                = 0;
    svar.GetInt("FrameTracker.DrawCurrentConnects", 1)  = 0;
    svar.GetInt("FrameTracker.DrawMatchedPoints", 1)    = 0;
    svar.GetInt("Map.DrawDataArea", 1)                  = 0;
    svar.GetInt("Map.DrawGPSPath", 1)                   = 1;
    svar.GetInt("Map.DrawKFRect", 1)                    = 0;
    svar.GetInt("Map.DrawLocalPoints", 1)               = 0;
    svar.GetInt("Map.DrawPoints", 1)                    = 0;
    svar.GetInt("Map.DrawKeyframes", 1)                 = 0;
    svar.GetInt("Map.DrawTexture", 1)                   = 0;

    svar.GetInt("VideoFrame.DrawTexture", 1)            = 0;

    svar.ParseLine("Map2DPrepare");
}

void GCS_MainWindow::action_SLAM_FollowCamera(void)
{
    int& followCamera = svar.GetInt("FrameTracker.FollowCamera", 0);
    followCamera = !followCamera;
}

void GCS_MainWindow::action_SLAM_FitGPS(void)
{
    svar.GetInt("KeyFrameHandle.FitGPSRequested", 0)    = 2;
    svar.GetInt("GlobalBundleRequested")                = 1;
}




void GCS_MainWindow::action_GCS_setGCS(void)
{
    if( m_uasManager == NULL ) return;

    UAS_Base *u = m_uasManager->get_active_gcs();
    if( u == NULL ) return;

    internals::PointLatLng               homePos;
    double                               homeAlt;

    mavlink_gps_raw_int_t  msg_gps_raw;
    mavlink_message_t      msg_mavlink;

    m_mapView->getGCS(homePos, homeAlt);

    // send GCS position
    {
        msg_gps_raw.time_usec           = tm_get_us();
        msg_gps_raw.lat                 = (int)( homePos.Lat()*1e7 );
        msg_gps_raw.lon                 = (int)( homePos.Lng()*1e7 );
        msg_gps_raw.alt                 = (int)( homeAlt * 1000.0 );
        msg_gps_raw.eph                 = 0;
        msg_gps_raw.epv                 = 0;
        msg_gps_raw.vel                 = 0;
        msg_gps_raw.fix_type            = 0;
        msg_gps_raw.satellites_visible  = 0;

        mavlink_msg_gps_raw_int_encode(u->gcsID, u->gcsCompID,
                                       &msg_mavlink, &msg_gps_raw);
        u->sendMavlinkMsg(&msg_mavlink);

        dbg_pi("[GCS] set pos: %f - %f (%f)\n",
               homePos.Lat(), homePos.Lng(), homeAlt);
    }
}

void GCS_MainWindow::action_GCS_setYawOffset(void)
{
    bool ok = true;

    // get value from configure file
    double GCS_yaw_offset = m_conf->value("GCS.yawOffset", 0).toDouble();

    // get user input
    double d = QInputDialog::getDouble(this,
                                tr("Set ATA yaw offset"),
                                tr("Angle (DEG):"),
                                GCS_yaw_offset, -360, 360,
                                1,
                                &ok);
    if ( ok ) GCS_yaw_offset = d;
    else return;

    // update new value to configure file
    m_conf->setValue("GCS.yawOffset", GCS_yaw_offset);
    m_conf->sync();

    // set & send to ATA
    if( m_uasManager != NULL ) {
        UAS_GCS *gcs = m_uasManager->get_active_gcs();

        if( gcs != NULL ) {
            gcs->ataYawOffset = GCS_yaw_offset;
            gcs->send_ATA_yawOffset();

            dbg_pi("[GCS] set yaw offset: %f", GCS_yaw_offset);
        }
    }
}

void GCS_MainWindow::action_GCS_testATA(void)
{
    static int wpIdx = 0;

    if( m_uasManager == NULL ) return;

    UAS_Base *u = m_uasManager->get_active_gcs();
    if( u == NULL ) return;

    QMap<int, mapcontrol::WayPointItem*> wpAll;
    mapcontrol::WayPointItem*            wp;
    internals::PointLatLng               homePos;
    double                               homeAlt;
    int                                  wpN;

    mavlink_gps_raw_int_t  msg_gps_raw;
    mavlink_message_t      msg_mavlink;


    wpAll = m_mapView->WPAll();
    m_mapView->getGCS(homePos, homeAlt);

    wpN = wpAll.size();
    if( wpN < 1 ) return;

    wp = wpAll[wpIdx%wpN];

    // send home position
    if( wpIdx%wpN == 0 ) {
        msg_gps_raw.time_usec           = tm_get_us();
        msg_gps_raw.lat                 = (int)( homePos.Lat()*1e7 );
        msg_gps_raw.lon                 = (int)( homePos.Lng()*1e7 );
        msg_gps_raw.alt                 = (int)( homeAlt * 1000.0 );
        msg_gps_raw.eph                 = 0;
        msg_gps_raw.epv                 = 0;
        msg_gps_raw.vel                 = 0;
        msg_gps_raw.fix_type            = 0;
        msg_gps_raw.satellites_visible  = 0;

        mavlink_msg_gps_raw_int_encode(u->gcsID, u->gcsCompID,
                                       &msg_mavlink, &msg_gps_raw);
        u->sendMavlinkMsg(&msg_mavlink);

        dbg_pi("[GCS] send GCS pos: %f - %f (%f)\n",
               homePos.Lat(), homePos.Lng(), homeAlt);
    }

    if( wpIdx%wpN > 0 ) {
        // send waypoint position
        msg_gps_raw.time_usec           = tm_get_us();
        msg_gps_raw.lat                 = (int)( wp->Coord().Lat()*1e7 );
        msg_gps_raw.lon                 = (int)( wp->Coord().Lng()*1e7 );
        msg_gps_raw.alt                 = (int)( wp->Altitude() * 1000.0 );
        msg_gps_raw.eph                 = 0;
        msg_gps_raw.epv                 = 0;
        msg_gps_raw.vel                 = 0;
        msg_gps_raw.fix_type            = 1;
        msg_gps_raw.satellites_visible  = 0;

        mavlink_msg_gps_raw_int_encode(u->gcsID, u->gcsCompID,
                                       &msg_mavlink, &msg_gps_raw);
        u->sendMavlinkMsg(&msg_mavlink);

        dbg_pi("[GCS] send wp[%4d]: %f - %f (%f)\n",
               wpIdx%wpN, wp->Coord().Lat(), wp->Coord().Lng(), wp->Altitude());
    }

    wpIdx ++;
}


void GCS_MainWindow::action_GCS_rcON(void)
{
    UAS_GCS *gcs = NULL;

    if( m_uasManager != NULL ) gcs = m_uasManager->get_active_gcs();
    if( gcs == NULL ) return;

    gcs->sendOptions(4, 1);
    dbg_pi("[GCS] RC to MAV: ON");

    svar.GetInt("UAS.GCS.rcToMAV", 1) = 1;
}

void GCS_MainWindow::action_GCS_rcOFF(void)
{
    UAS_GCS *gcs = NULL;

    if( m_uasManager != NULL ) gcs = m_uasManager->get_active_gcs();
    if( gcs == NULL ) return;

    // confirm turn-off RC
    {
        QMessageBox msgbox(QMessageBox::Question,
                           "Confirm", "Are you sure turn-off RC",
                           QMessageBox::Yes | QMessageBox::No);
        int v = msgbox.exec();
        if( v != QMessageBox::Yes ) return;
    }

    gcs->sendOptions(4, 0);
    dbg_pi("[GCS] RC to MAV: OFF");

    svar.GetInt("UAS.GCS.rcToMAV", 1) = 0;
}



void GCS_MainWindow::action_Test_saveVideo(void)
{
    svar.GetInt("FastGCS.Video.AutoSave", 1) = 0;
    svar.GetInt("FastGCS.Video.SaveOneShot", 0) = 1;
}

void GCS_MainWindow::action_Test_sendMission(void)
{
    UAS_Base *u = NULL;
    if( m_uasManager != NULL ) u = m_uasManager->get_active_mav();

    // get mission transfer server
    if( ! svar.exist("MissionData_Transfer.server.ptr") ) {
        dbg_pe("No MissionData_Transfer exist!");
        return;
    }

    MissionData_Transfer *md = (MissionData_Transfer*) svar.GetPointer("MissionData_Transfer.server.ptr", NULL);
    if( md == NULL ) {
        dbg_pe("No MissionData_Transfer exist!");
        return;
    }

    // get current waypoints
    AP_MissionArray wpa;
    m_mapView->getWaypoints(wpa);

    // send waypoints
    if( wpa.size() > 0 ) {
        for(int i=0; i<wpa.size(); i++) {
            AP_MissionItem *wp = wpa.get(i);

            MissionData_Simp m;

            m.missionNum    = wpa.size();
            m.missionIdx    = i;

            if( u != NULL ) m.uavID = u->ID;
            else            m.uavID = 0;

            m.lat           = wp->lat;
            m.lng           = wp->lng;
            m.alt           = wp->alt;
            m.missionType   = MissionData_Simp::MT_WAYPOINT;

            m.calcCRC();
            md->sendMission(&m);
        }
    } else {
        QMessageBox msgbox(QMessageBox::Warning,
                           "Warning", "No effective waypoints!");
        msgbox.exec();
    }
}

void GCS_MainWindow::action_Test_startMission(void)
{
    // default is to send missions to virtual UAV
    action_Test_sendMission();

    // send start UAV command
    UAS_Base *u = NULL;
    if( m_uasManager != NULL ) u = m_uasManager->get_active_mav();

    // get mission transfer server
    if( ! svar.exist("MissionData_Transfer.server.ptr") ) {
        dbg_pe("No MissionData_Transfer exist!");
        return;
    }

    MissionData_Transfer *md = (MissionData_Transfer*) svar.GetPointer("MissionData_Transfer.server.ptr", NULL);
    if( md == NULL ) {
        dbg_pe("No MissionData_Transfer exist!");
        return;
    }

    // get current waypoints
    AP_MissionArray wpa;
    m_mapView->getWaypoints(wpa);

    // get start waypoint index
    bool ok = true;
    int mi = QInputDialog::getInt(this,
                                  tr("Start waypoint index"),
                                  tr("Mission index:"),
                                  1,                        // default value
                                  0, 99999,                 // min/max
                                  1,                        // step
                                  &ok);

    // send mission command
    if( ok ) {
        MissionData_Simp m;

        m.missionNum    = wpa.size();
        m.missionIdx    = mi;

        if( u != NULL ) m.uavID = u->ID;
        else            m.uavID = 0;

        m.missionType   = MissionData_Simp::MT_START_MISSION;

        m.calcCRC();
        md->sendMission(&m);
    }
}

void GCS_MainWindow::action_Test_stopMission(void)
{
    UAS_Base *u = NULL;
    if( m_uasManager != NULL ) u = m_uasManager->get_active_mav();

    // get mission transfer server
    if( ! svar.exist("MissionData_Transfer.server.ptr") ) {
        dbg_pe("No MissionData_Transfer exist!");
        return;
    }

    MissionData_Transfer *md = (MissionData_Transfer*) svar.GetPointer("MissionData_Transfer.server.ptr", NULL);
    if( md == NULL ) {
        dbg_pe("No MissionData_Transfer exist!");
        return;
    }

    // get current waypoints
    AP_MissionArray wpa;
    m_mapView->getWaypoints(wpa);

    // send mission item
    {
        MissionData_Simp m;

        m.missionNum    = wpa.size();
        m.missionIdx    = 0;

        if( u != NULL ) m.uavID = u->ID;
        else            m.uavID = 0;

        m.missionType   = MissionData_Simp::MT_STOP_MISSION;

        m.calcCRC();
        md->sendMission(&m);
    }
}

void GCS_MainWindow::action_Test_execCommand(void)
{
    VirtualUAV_Manager *vuav_manager = m_dataManager->getVUAVManager();
    if( vuav_manager == NULL ) return;

    // get start waypoint index
    bool ok = true;
    QString cmd_ = QInputDialog::getText(this,
                                       "Execute command to virtual UAV",
                                       "Command:",
                                       QLineEdit::Normal,
                                       "",
                                       &ok);

    if( ok )
    {
        string cmd = cmd_.toStdString();
        VirtualUAV *vu = vuav_manager->getActiveUAV();
        if( vu )
        {
            dbg_pi("exe command: %s\n", cmd.c_str());
            vu->executeCommand(cmd);
        }
    }
}


void GCS_MainWindow::action_Help_about(void)
{
    UI_diagAbout diagAbout;

    diagAbout.exec();
}





void GCS_MainWindow::keyPressEvent(QKeyEvent *event)
{
    int     key;

    key  = event->key();

    if( key == Qt::Key_C ) {
        //clearPos();
    }
}

void GCS_MainWindow::mousePressEvent(QMouseEvent *event)
{

}

void GCS_MainWindow::resizeEvent(QResizeEvent *event)
{
    if( m_plot2D != NULL ) m_plot2D->resizeCanvas();
}

void GCS_MainWindow::timerEvent(QTimerEvent *event)
{
    static ru64 tmLast = 0, tmNow;

    // get UAS manager
    if( m_uasManager == NULL ) return;

    // update video frame
    /*
    VideoPOS_Manager* vpm = (VideoPOS_Manager*) svar.GetPointer("VideoPOS_Manager.ptr", NULL);
    if( vpm != NULL ) {
        SPtr<VideoData> vd = vpm->pop();
        if( vd.get() != NULL ) {
            m_mapView->setVideoFrame(&(vd->img));
        }

        int nSkip = vpm->size() / 3;
        for(int i=0; i<nSkip; i++) vpm->pop();
    }
    */

    // update MAV information
    UAS_MAV *mav = m_uasManager->get_active_mav();
    if( mav != NULL ) {

        // get current time
        tmNow = tm_get_ms();

        // set window title
        QString ts;

        if( mav->link_connected() )
            ts = m_sbTitleString + " - Link Connected";
        else
            ts = m_sbTitleString + " - Link Lost";
        setWindowTitle(ts);

        // update widgets
        m_FlightIns->setData(mav->roll, mav->pitch, mav->yaw,
                             mav->gpAlt, mav->gpH);

        if( m_plot2D != NULL ) m_plot2D->setAttitude(mav->roll, mav->pitch, mav->yaw);
        if( m_plot3D != NULL ) m_plot3D->setAttitude(mav->roll, mav->pitch, mav->yaw);
        if( m_ahrsViewer != NULL ) m_ahrsViewer->set_angles(mav->yaw, mav->pitch, mav->roll);

        // set home position
        if( mav->homeSetted == 1 && mav->homePosReaded == 0 && mav->gpsFixType >= 3 ) {
            internals::PointLatLng p(mav->homeLat, mav->homeLng);
            m_mapView->setHome(p, mav->alt);

            mav->homePosReaded = 1;
        }

        // add track to 2D/3D plot
        if( tmNow - tmLast > 100 ) {
            double      dx, dy, dz;

            // detect new position
            if( mav->homeSetted == 1 && mav->gpsFixType >= 3 ) {
                // calc offset from home point
                calcLngLatDistance(mav->homeLng, mav->homeLat,
                                   mav->lon, mav->lat,
                                   dx, dy);
                dz = mav->gpH - mav->homeH;

                if( m_plot2D != NULL ) m_plot2D->addPoint(dx, dy, dz);
                if( m_plot3D != NULL ) m_plot3D->addPoint(dx, dy, dz);

                // set map view
                mapcontrol::UAVItem *u = m_mapView->GetUAV(0);
                if( u == NULL ) {
                    u = m_mapView->AddUAV(0);

                    // FIXME: this usage failed!
                    //u->SetAutoSetDistance(svar.GetDouble("MapWidget.AutoReachDistance", 5.0, Svar::UPDATE));

                    u->SetAutoSetDistance(svar.GetDouble("MapWidget.AutoReachDistance", 5.0));
                }

                internals::PointLatLng p(mav->lat, mav->lon);
                u->SetUAVPos(p, mav->alt);
                u->SetUAVHeading(mav->yaw);
            }

            // set last time
            tmLast = tmNow;
        }
    }

    // update GCS information
    UAS_GCS *gcs = m_uasManager->get_active_gcs();
    if( gcs != NULL ) {
        // set GCS position
        if( gcs->homeSetted == 1 && gcs->homePosReaded == 0 && gcs->gpsFixType >= 3 ) {
            internals::PointLatLng p(gcs->homeLat, gcs->homeLng);
            m_mapView->setGCS(p, gcs->alt);

            gcs->homePosReaded = 1;
        }

        // set ATA yaw offset
        if( gcs->ataYawOffset_setted == 0 ) {
            // get value from configure file
            gcs->ataYawOffset = m_conf->value("GCS.yawOffset", 0).toDouble();

            // send to ATA
            gcs->send_ATA_yawOffset();
        }
    }

    // redraw widgets
    if( m_plot2D != NULL ) m_plot2D->plot();
    if( m_plot3D != NULL ) m_plot3D->plot();

    // update list
    {
        String2StringMap lm;
        m_infoList->beginSetData();
        m_uasManager->gen_listmap_important(lm);

        double lat, lng, zoom;
        lat  = svar.GetDouble("FastGCS.MapWidget.curLat", 0);
        lng  = svar.GetDouble("FastGCS.MapWidget.curLng", 0);
        zoom = svar.GetDouble("FastGCS.MapWidget.curZoom", 10);
        lm["Map"] = trim(fmt::sprintf("%10.6f, %10.6f : %d", lng, lat, (int)zoom));

        m_infoList->setData(lm);
        m_infoList->endSetData();

        m_infoList->listReload();
    }

    // do followMe
    if( SvarWithType<GPS_Reader*>::instance().exist("GPS_Reader.ptr") ) {
        GPS_Reader *gpsReader = SvarWithType<GPS_Reader*>::instance()["GPS_Reader.ptr"];
        if( gpsReader != NULL ) {
            POS_Data pd;

            double &tmLast = svar.GetDouble("IPC.followMe.tmLast", 0.0);
            double tmNow = tm_getTimeStamp();

            // get new position
            if( gpsReader->getData(pd) == 0 ) {
                if( tmNow - tmLast > svar.GetDouble("FastGCS.followMe.updatePeriod", 1.5) ) {
                    tmLast = tmNow;

                    if( mav != NULL && pd.posAvaiable ) {
                        dbg_pi("mobileGPS: nSat = %2d, HDOP = %6.1f", pd.nSat, pd.HDOP);
                        m_mapView->setGuidedTarget(pd.lat, pd.lng);

                        //FIXME: originally guided for APM
                        if( mav->customMode == MFM_AUTO_FOLLOW_TARGET ) {
                            // set initial height
                            if( !mav->m_guidedControl.posSetted ) {
                                double h;
                                if( mav->gpH < svar.GetDouble("FastGCS.followMe.minH", 5) )
                                    h = 5;
                                else
                                    h = mav->gpH;

                                mav->m_guidedControl.setPosition(mav->gpLat, mav->gpLon, h);
                            }

                            // set real position & height
                            mav->m_guidedControl.setPosition(pd.lat, pd.lng, mav->m_guidedControl.alt);
                            mav->setGuidedTarget(pd.lat, pd.lng, mav->m_guidedControl.alt,
                                                 UAS_MAV::GCS_FOLLOWME);
                        }
                    }
                }
            }

            // show lost signal message
            if( fabs(tmLast) > 1e-7 ) {
                if( tmNow - tmLast > 4.0 ) dbg_pe("Lost mobileGPS!");
            }
        }
    }

    // do joystick control for GuidedFlight
    pi::HAL_JSComobo* js = m_dataManager->getJoystick();
    if( js->isOpened() && mav != NULL ) {
        UAS_MAV::GuidedControl *gc = &(mav->m_guidedControl);

        // read joystick values
        double tmNow = tm_getTimeStamp();

        JS_Val jsv;
        int r = js->read(&jsv);
        if( r != 0 ) memset(&jsv, 0, sizeof(JS_Val));
        jsv.AXIS[1] = -jsv.AXIS[1];                     // FIXME: inverse pitch value

        //FIXME: GUIDED mode for APM
        if( mav->customMode == MFM_STABILIZED && r == 0 ) {
            if( svar.GetInt("UAS.MAV.GuidedFlight.showJS", 0) ) {
                printf("AXIS: ");
                for(int i=0; i<8; i++) printf("%5.1f ", jsv.AXIS[i]);
                printf("\n\n");
            }

            // set initial value
            if( gc->tmLastAction == 0 ) {
                gc->tmLastAction = tmNow;
                gc->tmLastInput = tmNow;

                gc->setPosition(mav->gpLat, mav->gpLon, mav->gpH);
            }

            // update new positon
            double dtInput = tmNow - gc->tmLastInput;
            gc->tmLastInput = tmNow;

            if( dtInput < 2*m_systemRefreshInterval ) {
                double vh, vv;
                double dx, dy, dh;

                // calculate new position
                vh = svar.GetDouble("UAS.MAV.GuidedFlight.speedH", 5.0);
                vv = svar.GetDouble("UAS.MAV.GuidedFlight.speedV", 2.5);

                dx =  vh * jsv.AXIS[0] * dtInput;
                dy =  vh * jsv.AXIS[1] * dtInput;
                dh =  vv * jsv.AXIS[2] * dtInput;

                double nlat, nlng, nalt;
                calcLngLatFromDistance(gc->lng, gc->lat, dx, dy,
                                       nlng, nlat);
                nalt = gc->alt + dh;

                // limit min height
                double minHeight = svar.GetDouble("UAS.MAV.GuidedFlight.minHeight", 5);
                if( nalt < minHeight ) nalt = minHeight;

                // redraw new position
                m_mapView->setGuidedTarget(nlat, nlng);
                gc->setPosition(nlat, nlng, nalt);
            }


            // send target to MAV
            double dtAction = tmNow - gc->tmLastAction;
            if( dtAction > svar.GetDouble("UAS.MAV.GuidedFlight.ControlInterval", 1.0) ) {
                mav->setGuidedTarget(gc->Lat(), gc->Lng(), gc->Alt(), UAS_MAV::GCS_JOYSTICK);
                gc->tmLastAction = tmNow;
            }
        }
    }
}

void GCS_MainWindow::closeEvent(QCloseEvent *event)
{
    // confirm quit
    QMessageBox msgbox;
    msgbox.setWindowTitle("Confirm");
    msgbox.setText("Are you sure to quit?");
    msgbox.setStandardButtons( QMessageBox::Cancel | QMessageBox::Ok );
    msgbox.setDefaultButton(QMessageBox::Cancel);
    msgbox.show();
    msgbox.move(geometry().center().x()-msgbox.size().width()/2,
                geometry().center().y()-msgbox.size().height()/2);

    if( msgbox.exec() == QMessageBox::Ok ) {
        m_uasManager = NULL;
        event->accept();
    } else{
        event->ignore();
    }
}


void GCS_MainWindow::mw_onMapDrag(void)
{
    internals::PointLatLng pos;

    pos = m_mapView->CurrentPosition();

    m_conf->setValue("MapWidget.lastPos.lat", pos.Lat());
    m_conf->setValue("MapWidget.lastPos.lng", pos.Lng());

    m_conf->sync();
}

void GCS_MainWindow::mw_zoomChanged(int newZoom)
{
    m_conf->setValue("MapWidget.lastZoom", newZoom);
    svar.GetDouble("FastGCS.MapWidget.curZoom", 10) = newZoom;

    m_conf->sync();
}



int GCS_MainWindow::setDataManager(DataManager *dm)
{
    m_dataManager = dm;

    setUASManager(dm->getUASManager());
    setComManager(dm->getComManager());

    return 0;
}


int GCS_MainWindow::setUASManager(UAS_Manager *u)
{
    m_uasManager = u;
    m_mapView->setUASManager(u);
    m_flightControl->setUASManager(u);

    return 0;
}

int GCS_MainWindow::setComManager(ComManager *cm)
{
    m_comManager = cm;

    return 0;
}


int GCS_MainWindow::loadMesh(void)
{
    int ret1, ret2, ret;

    string dp = svar.GetString("FastGCS.DataPath", "./Data/FastGCS");
    string fn_mesh = path_join(dp, "other/airplane.off");

    if( m_ahrsViewer != NULL ) ret1 = m_ahrsViewer->load_mesh(fn_mesh);
    if( m_plot3D != NULL )     ret2 = m_plot3D->load_mesh(fn_mesh);

    if( ret1 > ret2 ) ret = ret1;
    else              ret = ret2;

    return ret;
}
